#include "physics.h"
#include "actor.h"
#include "..\common\def.h"
#include "..\common\util.h"
#include "..\engine\error.h"
#include "..\engine\engine.h"
#include "..\engine\time.h"

#include "Physics\Dynamics\Entity\hkpRigidBodyCinfo.h"
#include "Physics\Dynamics\Entity\hkpRigidBody.h"
#include "Physics\Dynamics\Collide\hkpResponseModifier.h"
#include "Physics\Utilities\Dynamics\Inertia\hkpInertiaTensorComputer.h"
#include "Physics\Collide\Shape\Compound\Collection\hkpShapeCollection.h"
#include "Physics\Collide\Shape\Compound\Collection\List\hkpListShape.h"
#include "Physics\Collide\Shape\Convex\Sphere\hkpSphereShape.h"
#include "Physics\Collide\Shape\Convex\Capsule\hkpCapsuleShape.h"
#include "Physics\Collide\Shape\Convex\Box\hkpBoxShape.h"
#include "Physics\Collide\Shape\Misc\Transform\hkpTransformShape.h"
#include "Common\Base\Types\Geometry\hkStridedVertices.h"
#include "Physics\Collide\Shape\Convex\ConvexVertices\hkpConvexVerticesShape.h"
#include "Physics\Collide\Shape\Compound\Collection\ExtendedMeshShape\hkpExtendedMeshShape.h"
#include "common\base\math\vector\hkVector4.h"
#include "Common\Base\Types\hkBaseTypes.h"
#include "Common\Base\Math\Matrix\hkTransform.h"

#include <Common/Base/Reflection/hkClass.h>
#include <Common/Serialize/Util/hkLoader.h>
#include <Common/Serialize/Util/hkRootLevelContainer.h>
#include <Common/Base/Reflection/Registry/hkTypeInfoRegistry.h>
#include <Physics/Utilities/Serialize/hkpPhysicsData.h>
#include <Common/Base/Container/LocalArray/hkLocalBuffer.h>
#include <Common/SceneData/Scene/hkxScene.h>


/**************************************
 *
 **************************************/
CActor::ActorData::ActorData()
{
	
	initalTransform = Matrix4f::identity(); 
	mass = 1.0f;
	linearDampening = 0.0f;
	angularDampening = 0.05f;
	
	maxLinearSpeed = 100.0f;
	maxAngularSpeed = 100.0f;
	maxForce = 1000.0f;
	maxSlope = HK_REAL_PI / 2.0f;
	
	restitution = 0.5f;
	friction = 0.5f;
};

/**************************************
 *
 **************************************/
CActor::CActor(CPhysics* const pPhysics)
{
	m_AllowSetStill = false;
	m_Transform = Matrix4f::identity();
	m_pPhysics = pPhysics;
	m_pActor = NULL;
}

/**************************************
 *
 **************************************/
CActor::~CActor()
{
	Release();
}

/**************************************
 *Retrieves data from the physics engine
 *that is calculated during simulation for
 *client use
 **************************************/
void CActor::Simulate()
{
	if(m_pActor)
	{
		m_Transform = ToMatrix4f( m_pActor->getTransform());
		m_Velocity = ToVector3f(m_pActor->getLinearVelocity());
	}
}

/**************************************
 *Releases all resources allocated by
 *this actor instance
 **************************************/
HRESULT CActor::Release()
{
	HRESULT result = S_OK;

	if(m_pActor)
	{
		
		for(uint i = 0, n = m_CollisionListeners.Count(); i < n; i++) {
			m_pActor->removeCollisionListener(m_CollisionListeners[i]);
			EE_SAFEDELETE(m_CollisionListeners[i]);
		}

		m_pActor->activate();
		if(m_pPhysics->GetScene()->removeEntity(m_pActor) == false)
			result = E_FAIL;
		m_pActor = NULL;
	}

	m_Transform = Matrix4f::identity();
	return result;
}

/**************************************
 *Internal function for loading shape
 *data from file. Shape is also registerd
 *with physics engine for possible reuse
 **************************************/
HRESULT CActor::CreateShape(const string &filename, hkpShape** pOut)
{
	FILE* pFile = NULL;
	short header;
	short footer;
	unsigned int maxShapes = 100;

	try {

		if(ISNULL(pOut))
			throw EE_EXCEPTION("Bad parameter.");

		if(!filename.IsValid())
			throw EE_EXCEPTION("Invalid filename.");

		for(int i = 0, j = m_pPhysics->m_Shapes.Count(); i < j; i++)
		{
			hkpShape* pShape = m_pPhysics->m_Shapes[i];
			const char* shapeName = (const char*)pShape->m_userData;
			if(shapeName == filename.c_str())
			{
				*pOut = pShape;
				return S_OK;
			}
		}

		pFile = CEngine::GetInstance()->FindFile(filename);
		if(ISNULL(pFile))
			throw EE_EXCEPTION("Failed to locate or open file.");

		fread(&header, sizeof(short), 1, pFile);
		if(header != 200)
			throw EE_EXCEPTION("Invalid or corrupt file.");

		ArrayList<hkpShape*> shapes;

		fread(&header, sizeof(short), 1, pFile);
		while(header != 101)
		{
			if(maxShapes < 0)
			{
				EE_LOGERROR("Warning: too many physics shapes or corrupt file.");
				break;
			}
			maxShapes--;

			switch(header)
			{
			case 1:	//Box
				{		
					float dimensions[3] = {0};
					float mat[12] = {0};

					fread(mat, sizeof(float), 12, pFile);
					fread(dimensions, sizeof(float), 3, pFile);

					hkVector4 halfExtents(dimensions[0] * 0.5f,dimensions[1] * 0.5f,dimensions[2] * 0.5f);

					shapes.Add(new hkpTransformShape(new hkpBoxShape(halfExtents, 0), ToEEhkTransform(mat)));

				}break;
			case 2: //Sphere
				{
					float radius = 0;
					float mat[12] = {0};

					fread(mat, sizeof(float), 12, pFile);
					fread(&radius, sizeof(float), 1, pFile);
										
					shapes.Add(new hkpTransformShape(new hkpSphereShape(radius), ToEEhkTransform(mat)));

				}break;
			case 4: //Capsule
				{
					float radius = 0;
					float height = 0;
					float mat[12] = {0};

					fread(mat, sizeof(float), 12, pFile);
					fread(&radius, sizeof(float), 1, pFile);
					fread(&height, sizeof(float), 1, pFile);
					
					hkVector4 vA(0,height * 0.5,0);
					hkVector4 vB(0,height * -0.5,0);

					shapes.Add(new hkpTransformShape(new hkpCapsuleShape( vA, vB, radius), ToEEhkTransform(mat)));

				}break;
			case 3: //Convex
				{
					float mat[12] = {0};
					short faceCount;

					fread(mat, sizeof(float), 12, pFile);
					fread(&faceCount, sizeof(short), 1, pFile);

					if(faceCount > 0)
					{			
						
						struct vert{ float x; float y; float z; };
						int vertexCount = faceCount * 3;
						vert* pVertices = EE_NEW vert [vertexCount];
						hkUint16* pIndicies = EE_NEW hkUint16 [vertexCount];

						hkpExtendedMeshShape::TrianglesSubpart triangles;

						for(int i = 0; i < faceCount; i++)
						{
							fread(&pVertices[(i * 3) + 0], sizeof(vert), 1, pFile);
							fread(&pVertices[(i * 3) + 1], sizeof(vert), 1, pFile);
							fread(&pVertices[(i * 3) + 2], sizeof(vert), 1, pFile);
						
							pIndicies[(3*i)+0] = (3*i)+0;
							pIndicies[(3*i)+1] = (3*i)+2;
							pIndicies[(3*i)+2] = (3*i)+1;
						}

						triangles.m_numTriangleShapes = faceCount;
						triangles.m_vertexBase = (float*)pVertices;
						triangles.m_vertexStriding = sizeof(float) * 3;
						triangles.m_numVertices = vertexCount;

						triangles.m_indexBase = pIndicies;
						triangles.m_indexStriding = sizeof(hkUint16) * 3;
						triangles.m_stridingType = hkpExtendedMeshShape::INDICES_INT16;
						
						
						hkTransform transform = ToEEhkTransform(mat);
					//	transform.setIdentity();
						hkpExtendedMeshShape* pTriangleSoup = new hkpExtendedMeshShape();
						hkpTransformShape* pTransformShape = new hkpTransformShape(pTriangleSoup, transform);

						pTransformShape->setTransform(transform); //makes no difference as already pre transformed

						pTriangleSoup->addTrianglesSubpart(triangles);
						shapes.Add(pTransformShape);

						//pTransformShape->removeReference();
						pTriangleSoup->removeReference();

					}

				}break;
			default:
				EE_LOGERROR("Warning: unknown physics shape.");
			}

			fread(&header, sizeof(short), 1, pFile);
		}

		if(pFile)
			fclose(pFile);

		hkpListShape *pListShape = new hkpListShape(shapes.Data(), shapes.Count());
		pListShape->m_userData = (hkUlong) filename.c_str();
		m_pPhysics->m_Shapes.Add(pListShape);
		
		*pOut = pListShape;

		return S_OK;

	}catch(CError::CErrorData error)
	{
		if(pFile)
			fclose(pFile);

		CError::CError(error);
		return E_FAIL;
	}

	return S_OK;
}

/**************************************
 *Allocates physics resources which are
 *defined by data within the specified file
 *for use in the application
 **************************************/
HRESULT CActor::Create(const string& filename, CActor::ActorData actorData, CObject* pParent)
{
	string tm = CEngine::GetInstance()->FindFilepath(filename);
	hkLoader* m_loader = new hkLoader();

	hkString assetFile(tm.c_str());// = hkAssetManagementUtil::getFilePath("Resources/Physics/levels/twosided_test_level.hkx");
	hkRootLevelContainer* container = m_loader->load( assetFile.cString() );
	hkxScene* scene = reinterpret_cast<hkxScene*>( container->findObjectByType( hkxSceneClass.getName() ));

	hkpPhysicsData* physics = reinterpret_cast<hkpPhysicsData*>( container->findObjectByType( hkpPhysicsDataClass.getName() ));

	// Physics
	if (physics)
	{
		const hkArray<hkpPhysicsSystem*>& psys = physics->getPhysicsSystems();

		// Tie the two together
		for (int i=0; i<psys.getSize(); i++)
		{
			hkpPhysicsSystem* system = psys[i];
			//system->transform(TohkTransform(actorData.initalTransform));
			//system->setActive(true);
			m_pActor = system->getRigidBodies()[0];

			if(m_pActor->getMotionType() == hkpMotion::MotionType::MOTION_FIXED || m_pActor->getMotionType() == hkpMotion::MotionType::MOTION_KEYFRAMED)
				m_AllowSetStill = false;
			else
				m_AllowSetStill = true;

			// add the lot to the world
			m_pPhysics->GetScene()->addPhysicsSystem(system);
		}
	}

	hkTransform t = TohkTransform(actorData.initalTransform);
	if(t.isOk())
		m_pActor->setTransform(t);
	m_pActor->setUserData((hkUlong)pParent);
	return S_OK;
}

class UnaffectListener : public hkpCollisionListener
{ 
	public:
	hkpRigidBody *m_pSelf;
	hkpRigidBody *m_pOther;
	UnaffectListener(hkpRigidBody* pSelf, hkpRigidBody* pOther) : m_pSelf(pSelf), m_pOther(pOther)
	{	
		m_pSelf->addCollisionListener(this);
	}

	void contactPointAddedCallback(	hkpContactPointAddedEvent& event) {

	}

	void contactPointConfirmedCallback( hkpContactPointConfirmedEvent& event) {
		
	}

	void contactPointRemovedCallback( hkpContactPointRemovedEvent& event ) {

	}

	void contactProcessCallback( hkpContactProcessEvent& event ) {
		hkpRigidBody* bodyA, *bodyB;
		bodyA = static_cast<hkpRigidBody*>(event.m_collidableA->getOwner());
		bodyB = static_cast<hkpRigidBody*>(event.m_collidableB->getOwner());

		if(bodyB == m_pOther && bodyA == m_pSelf)
		{
			hkpConstraintOwner* owner = event.m_collisionData->m_constraintOwner;  

			// This is where the magic happens. By using hkVector4::getZero() as the inverse mass scaling 
			// for the other rigid body, we effectively make it fixed during the collision
			hkpResponseModifier::setInvMassScalingForContact( event.m_internalContactMgr,m_pSelf,
			*owner, hkVector4::getZero() );
		}
	}
};

class CollisionListener : public hkpCollisionListener
{ 
	public:
	hkpRigidBody *m_pActor;
	CObject *m_pObject;
	CollisionListener(CObject* pObject, hkpRigidBody* pObjectActor) : m_pActor(pObjectActor), m_pObject(pObject)
	{	
		m_pActor->addCollisionListener(this);
	}

	void contactPointAddedCallback(	hkpContactPointAddedEvent& event) {
	}

	void contactPointConfirmedCallback( hkpContactPointConfirmedEvent& event) {

		hkpRigidBody* pOtherBody = static_cast<hkpRigidBody*>(event.m_collidableA->getOwner());
		CObject* pOtherObject = NULL;
		hkContactPoint* pContactPoint = event.m_contactPoint;

		Vector3f contactPoint = ToVector3f(pContactPoint->getPosition());
		Vector3f contactNormal = ToVector3f(pContactPoint->getNormal());

		if(pOtherBody)
			pOtherObject = (CObject*)pOtherBody->getUserData();
		if(pOtherObject)
			pOtherObject->OnCollisionEvent(contactPoint, contactNormal, m_pObject);

		m_pObject->OnCollisionEvent(contactPoint, contactNormal * -1, pOtherObject);
		
	}

	void contactPointRemovedCallback( hkpContactPointRemovedEvent& event ) {

	}

	void contactProcessCallback( hkpContactProcessEvent& event ) {

	}
};

/**************************************
 *Enables client specification of and object
 *who's physical simulation does not impact
 *on this actors transformation/simulation
 **************************************/
void CActor::SetResponceToObject(CObject* pObject, const ResponceType& type) {

	switch(type) {
		case NORMAL: 
			{
				for(uint i = 0, n = m_pActor->getCollisionListeners().getSize(); i < n; i++) {
					
					UnaffectListener *pUnaffectListener = (UnaffectListener*)m_pActor->getCollisionListeners()[i];
					if(pUnaffectListener->m_pOther == pObject->m_pPhysics->m_pActor) {
						m_pActor->removeCollisionListener(pUnaffectListener);
					}
				}
			}break;
		case UNAFFECTED: 
			{
				if(pObject) {
					m_CollisionListeners.Add(new UnaffectListener(m_pActor, pObject->m_pPhysics->m_pActor));
				}
			}break;
	}
}

/**************************************
 *Enables client specification of a callback
 *which is called when the actor collides 
 *with some other actor
 **************************************/
void CActor::SetCollisionCallback(CObject* pObject) {

	m_CollisionListeners.Add(new CollisionListener(pObject, m_pActor));
}

/**************************************
 *Enables client specification of actor
 *tranform
 **************************************/
void CActor::SetTransform(const Matrix4f& transform)
{
	m_pActor->setTransform(TohkTransform(transform));
}

/**************************************
 *Enables client specification of actor
 *angular velocity
 **************************************/
void CActor::SetAngluarVelocity(const Vector3f& axisMagnitude)
{
	m_pActor->setAngularVelocity(hkVector4(axisMagnitude.x,axisMagnitude.y,axisMagnitude.z,0.0f));
}

/**************************************
 *Enables client specification of actor
 *linear velocity
 **************************************/
void CActor::SetLinearVelocity(const Vector3f& directionMagnitude)
{
	m_pActor->setLinearVelocity(hkVector4(directionMagnitude.x,directionMagnitude.y,directionMagnitude.z,0.0f));
}

/**************************************
 *Enables client specification of actor
 *linear dampening
 **************************************/
void CActor::SetLinearDamping(const float value)
{
	m_pActor->setLinearDamping(value);
}

/**************************************
 *Enables client specification of actor
 *instantaneous force
 **************************************/
void CActor::AddForce(const Vector3f& force)
{
	m_pActor->applyLinearImpulse(hkVector4(force.x,force.y,force.z,0.0f));
}

/**************************************
 *Enables client specification of actor
 *being still
 **************************************/
void CActor::SetStill(bool val)
{
	if(m_AllowSetStill)
		m_pActor->setMotionType(val ? hkpMotion::MOTION_FIXED : hkpMotion::MOTION_DYNAMIC);
}

/**************************************
 *Enables client specification of actors
 *angular dampening
 **************************************/
//void CActor::SetAngularDampening(const float value)
//{
//	m_pActor->setAngularDamping(value);
//}

/**************************************
 *Retrieves an engine compatible transformation
 *such that the objects simulation can be
 *observed within the application
 **************************************/
Matrix4f CActor::GetTransform() const
{
	return m_Transform;
}

/**************************************
 *Retrieves linear velocity of actor in
 *world space
 **************************************/
Vector3f CActor::GetLinearVelocity() const
{
	return m_Velocity;
}